//
// Created by lizhehao on 2/21/21.
//

//#define USE_CAMERA
//#define SAVE_VIDEO
//#define SAVESAVE_LEAF_IMG
// #define DEBUG
// #define DEBUG_LOG
// #define SHOW_RESULT
// #define SHOW_CIRCLE
// #define SHOW_ALL_CONTOUR
//#define RED
#define PREDICT
#define TRACK

#include "PowerRuneDetector.h"
#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/core/ocl.hpp>
#include <numeric>
#include "manifoldCommunication.h"
#include "minAreaRect.h"
#include<chrono>
using namespace cv;
using namespace std;

constexpr double PI = 3.141592653589793238462643;
inline double clip(double num, double min = -INFINITY, double max = INFINITY)
{
    return num < min ? min : (num < max ? num : max);
}

double getDistance(Point2f A, Point2f B)
{
    double dis;
    dis = pow((A.x - B.x), 2) + pow((A.y - B.y), 2);
    return sqrt(dis);
}

/*
 * 参考: http://blog.csdn.net/liyuanbhu/article/details/50889951
 * 通过最小二乘法来拟合圆的信息
 * pts: 所有点坐标
 * center: 得到的圆心坐标
 * radius: 圆的半径
 */
static bool CircleInfo2(std::deque<cv::Point2f>& pts, cv::Point2f& center, double& radius)
{
    center = cv::Point2d(0, 0);
    radius = 0.0;
    if (pts.size() < 3) return false;

    double sumX = 0.0;
    double sumY = 0.0;
    double sumX2 = 0.0;
    double sumY2 = 0.0;
    double sumX3 = 0.0;
    double sumY3 = 0.0;
    double sumXY = 0.0;
    double sumX1Y2 = 0.0;
    double sumX2Y1 = 0.0;
    const double N = (double)pts.size();
    for (int i = 0; i < pts.size(); ++i)
    {
        double x = pts.at(i).x;
        double y = pts.at(i).y;
        double x2 = x * x;
        double y2 = y * y;
        double x3 = x2 * x;
        double y3 = y2 * y;
        double xy = x * y;
        double x1y2 = x * y2;
        double x2y1 = x2 * y;

        sumX += x;
        sumY += y;
        sumX2 += x2;
        sumY2 += y2;
        sumX3 += x3;
        sumY3 += y3;
        sumXY += xy;
        sumX1Y2 += x1y2;
        sumX2Y1 += x2y1;
    }
    double C = N * sumX2 - sumX * sumX;
    double D = N * sumXY - sumX * sumY;
    double E = N * sumX3 + N * sumX1Y2 - (sumX2 + sumY2) * sumX;
    double G = N * sumY2 - sumY * sumY;
    double H = N * sumX2Y1 + N * sumY3 - (sumX2 + sumY2) * sumY;

    double denominator = C * G - D * D;
    if (std::abs(denominator) < DBL_EPSILON) return false;
    double a = (H * D - E * G) / (denominator);
    denominator = D * D - G * C;
    if (std::abs(denominator) < DBL_EPSILON) return false;
    double b = (H * C - E * D) / (denominator);
    double c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;

    center.x = a / (-2);
    center.y = b / (-2);
    radius = std::sqrt(a * a + b * b - 4 * c) / 2;
    return true;
}

//模板匹配
double PowerRuneDetector::TemplateMatch(cv::Mat image, cv::Mat tepl, cv::Point2f& point, int method)
{
    int result_cols = image.cols - tepl.cols + 1;
    int result_rows = image.rows - tepl.rows + 1;
    //    cout <<result_cols<<" "<<result_rows<<endl;
    cv::Mat result = cv::Mat(result_cols, result_rows, CV_32FC1);
    cv::matchTemplate(image, tepl, result, method);

    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc, Mat());

    switch (method)
    {
    case TM_SQDIFF:
    case TM_SQDIFF_NORMED:
        point = minLoc;
        return minVal;

    default:
        point = maxLoc;
        return maxVal;

    }
}

PowerRuneDetector::PowerRuneDetector(const double fps)
{
    int M = 2 * PI / 1.884 * fps + 1;
    for (int i = 0; i < M; i++)
    {
        this->real_spd.push_back(0.785 * sin(2 * PI * i / M) + 1.305);
    }
    //读取模板图片并resize
    for (int i = 1; i <= 8; i++)
    {
        this->templ[i] = imread("PowerRune/data/template/template" + to_string(i) + ".jpg", IMREAD_GRAYSCALE);
        //templ[i] = imread("D:/PowerRune/RM19windmillDemo-master/template/leaf" + to_string(i) + ".jpg", IMREAD_GRAYSCALE);
        resize(this->templ[i], this->templ[i], Size(42, 20));
    }
    this->is_tracking = false;
    this->fps = fps;
    this->template_max = 0;
    this->template_dir = 1;
    //this->thr = 0.45;
    this->thr = 0.57;
}

void PowerRuneDetector::setPowerRuneColor(const string& color)
{
    if(color == "blue")
        this->PRcolor = PowerRuneColor::Blue;
    else
        this->PRcolor = PowerRuneColor::Red;
}

int PowerRuneDetector::Compare(double* spd, int n)
{
    int M = 2 * PI / 1.884 * this->fps + 1;
    double* r = new double[M + n - 1];

    double min = INFINITY;
    int idx = 0;
    for (int i = 0; i < M + n - 1; i++)
    {
        r[i] = 0;
        for (int j = 0; j < n; j++)
        {
            r[i] += pow((spd[j] - this->real_spd[(i + j) % M]), 2);
        }
        if (min > r[i])
        {
            min = r[i];
            idx = i;
        }
    }
    delete r;
    return (idx + n) % M;
}

void PowerRuneDetector::predict(bool mode, double delay, deque<Point2f> sampled_pts, Point2f& p, bool clearflag = false)
{
    static bool first_in = 1;
    double sampled_spd = 0;
    static RotateDirection direction = RotateDirection::clockwise;//1顺时针，0逆时针
    static double predicted_angle = 0;
    if (clearflag) first_in = 1; 

    // printf("1.\n");
    if (first_in) // 第一次预测
    {   //确定旋转方向 
        double theta1 = acos((sampled_pts[sampled_pts.size() - 3].x - this->cc.x) / this->R);
        double theta2 = acos((sampled_pts[sampled_pts.size() - 1].x - this->cc.x) / this->R);
        if (sampled_pts[sampled_pts.size() - 3].y - this->cc.y > 0) theta1 = -theta1;
        if (sampled_pts[sampled_pts.size() - 1].y - this->cc.y > 0) theta2 = -theta2;

        if (theta1 > theta2 || theta2 - theta1 > 3.14) direction = RotateDirection::clockwise;
        else direction = RotateDirection::counterclockwise;
    }

    if (mode) // mode的含义？
    {
        static int idx=0;
        if (first_in)
        {
            double* first_sampled_spd = new double[sampled_pts.size() - 1];
            for (int i = 1; i < sampled_pts.size(); i++)
            {
                first_sampled_spd[i - 1] = getDistance(sampled_pts[i], sampled_pts[i - 1]) / this->R * this->fps;
            }
            idx = this->Compare(first_sampled_spd, sampled_pts.size() - 1);
        }
        else
        {
            int M = 2 * PI / 1.884 * this->fps + 1;
            idx = (idx + 1) % M;
            double distance = getDistance(sampled_pts[sampled_pts.size() - 2], sampled_pts[sampled_pts.size() - 1]);
            if (distance > 2 * this->R * sin(3.1 / 5))
                sampled_spd = predicted_angle / delay;
            else sampled_spd = distance / this->R * this->fps;
            
            double min_gap = INFINITY;
            int min_idx = idx;
            double gap[5];
            for (int i = 0; i < 5; i++)
            {
                gap[i] = abs(sampled_spd - this->real_spd[(idx + 2 - i + M) % M]);
                if (min_gap > gap[i])
                {
                    min_gap = gap[i];
                    min_idx = (idx + 2 - i + M) % M;
                }
            }
            idx = min_idx;
        }
        predicted_angle = this->real_spd[idx] * delay;

    }
    else
    {
        if (first_in)
        {
            for (int i = 1; i < sampled_pts.size(); i++)
            {
                predicted_angle += getDistance(sampled_pts[i], sampled_pts[i - 1]);
            }
            predicted_angle = predicted_angle / (sampled_pts.size() - 1) / this->R * this->fps * delay;
        }
        else
        {
            // printf("5.\n");
            //if (getDistance(sampled_pts[sampled_pts.size() - 2], sampled_pts[sampled_pts.size() - 1]) < 2 * this->R * sin(3.1 / 5))
            {
                double arc_len = 0.0;
                // printf("sampled_pts.size(): %ld\n", sampled_pts.size());
                // printf("this->fps: %lf\n", this->fps);
                for(int i = 0; i < sampled_pts.size() - 1;)
                {
                    arc_len += getDistance(sampled_pts[sampled_pts.size() - i - 1], sampled_pts[sampled_pts.size() - i - 2]);
                    i+= 2; 
                }
                predicted_angle = arc_len / this->R * this->fps / sampled_pts.size() / 2.  * delay;
                //cout<<"angle: "<< predicted_angle<<endl;

            }
            // printf("6.\n");
                //predicted_angle = (0.3 * getDistance(sampled_pts[sampled_pts.size() - 1], sampled_pts[sampled_pts.size() - 2]) / this->R * this->fps * delay + 0.7 * predicted_angle);
        }
    }
    //double theta = acos((sampled_pts[sampled_pts.size() - 1].x - this->cc.x) / this->R);
    double theta = -atan2(sampled_pts[sampled_pts.size() - 1].y - this->cc.y, sampled_pts[sampled_pts.size() - 1].x - this->cc.x);
    //if (sampled_pts[sampled_pts.size() - 1].y - this->cc.y > 0) theta = -theta;
    if (direction == RotateDirection::clockwise) theta -= predicted_angle;
    else theta += predicted_angle;
    //printf("[%f, %f], %f %f %f\n", sampled_pts[sampled_pts.size() - 1].x, sampled_pts[sampled_pts.size() - 1].y, theta, predicted_angle, this->R);
    p.x = this->cc.x + this->R * cos(theta);
    p.y = this->cc.y - this->R * sin(theta);
    first_in = 0;
    // printf("7.\n");
}

void PowerRuneDetector::preprocess(Mat& img)
{
    //分割颜色通道
    vector<Mat> imgChannels;
    split(img, imgChannels); // BGR 
    //获得目标颜色图像的二值图，阈值需要根据实际情况调整
    Mat midImage2;
    if(this->PRcolor == PowerRuneColor::Red)
    {
        midImage2= imgChannels.at(2) - imgChannels.at(0);
        threshold(midImage2, midImage2, 100, 255, THRESH_BINARY);
    }
    else
    { 
        midImage2 = imgChannels.at(1) - imgChannels.at(2);
        threshold(midImage2, midImage2, 40, 255, THRESH_BINARY);
    }

#ifdef DEBUG
        namedWindow("1. origin", WINDOW_NORMAL);
        imshow("1. origin", img);
        namedWindow("2. binary", WINDOW_NORMAL);
        imshow("2. binary", midImage2);
#endif 

    int structElementSize = 1;
    Mat element = getStructuringElement(MORPH_RECT, Size(2 * structElementSize + 1, 2 * structElementSize + 1), Point2f(structElementSize, structElementSize));
    //膨胀
    dilate(midImage2, midImage2, element);
    //开运算，消除扇叶上可能存在的小洞
    structElementSize = 1;//1
    //structElementSize = 1;
    element = getStructuringElement(MORPH_RECT, Size(2 * structElementSize + 1, 2 * structElementSize + 1), Point2f(structElementSize, structElementSize));
    morphologyEx(midImage2, midImage2, MORPH_CLOSE, element);

    #ifdef DEBUG
        namedWindow("3. dilate", WINDOW_NORMAL);
        imshow("3. dilate", midImage2);
#endif
    img = midImage2;
}

bool getLeafImage(Point2f* P, double width, double height, Mat& midImage2, Mat& leaf_img)
{
    Point2f srcRect[4];
    Point2f dstRect[4];

    if (width > height)
    {
        srcRect[0] = P[0];
        srcRect[1] = P[1];
        srcRect[2] = P[2];
        srcRect[3] = P[3];
    }
    else
    {
        swap(width, height);
        srcRect[0] = P[1];
        srcRect[1] = P[2];
        srcRect[2] = P[3];
        srcRect[3] = P[0];
    }

    dstRect[0] = Point2f(0, 0);
    dstRect[1] = Point2f(width, 0);
    dstRect[2] = Point2f(width, height);
    dstRect[3] = Point2f(0, height);
    // 应用透视变换，矫正成规则矩形
    Mat transform = getPerspectiveTransform(srcRect, dstRect);
    Mat inverse_transform = getPerspectiveTransform(dstRect, srcRect);
    Mat perspectMat;
    // warpPerspective(midImage2, perspectMat, transform, Size(midImage2.rows*1.5, midImage2.cols*1.5));
    warpPerspective(midImage2, perspectMat, transform, Size(width,height));
#ifdef DEBUG
    namedWindow("4. warpdst", WINDOW_NORMAL);
    imshow("4. warpdst", perspectMat);
#endif
    // 提取扇叶图片
    leaf_img = perspectMat(Rect(0, 0, clip(width, 0, perspectMat.cols), clip(height, 0, perspectMat.rows)));
    if(leaf_img.empty())
        return false;
    else return true; 
}

void PowerRuneDetector::leafImageTemplateMatch(Mat& leaf_img)
{
    cv::Point2f matchLoc;
    double value;
    Mat tmp1;
    resize(leaf_img, tmp1, Size(42, 20));

    //扇叶叶片的模板匹配
    this->template_max = 0;
    for (int j = 1; j <= 6; j++)
    {
        value = this->TemplateMatch(tmp1, this->templ[j], matchLoc, TM_CCOEFF_NORMED);
        if (this->template_max < value) this->template_max = value; // find max 
        if (this->template_max > this->thr)
        {
            if (j > 3) this->template_dir = 1;
            else this->template_dir = 0;
            break;
        }
    }
}

void PowerRuneDetector::getPowerRuneArmorBoxCenter(Vec4i& hierarchy, vector<vector<Point>>& contours2, RotatedRect& rect_tmp2,
                                                     Point2f& centerP, Point2f& offset, Mat& img, Mat& srcImage)
{
    //查找装甲板
#ifdef TRACK
    offset = Point2f(this->bbox.x, this->bbox.y);
#endif 

    //Note: hierarchy = hierarchys[i]
    if (hierarchy[2] >= 0) //内层的装甲板
    { 
        RotatedRect rect_tmp = minAreaRect(contours2[hierarchy[2]]);
        
        Point2f Pnt[4];
        rect_tmp.points(Pnt);
#ifdef TRACK
        if (this->is_tracking) centerP = rect_tmp.center + offset;
        else centerP = rect_tmp.center;
#else
        centerP = rect_tmp.center;
#endif
        // cout << centerP << endl;  // For Debug

#ifdef SHOW_RESULT
        for (int j = 0; j < 4; ++j)
        {
            line(img, Pnt[j], Pnt[(j + 1) % 4], Scalar(0, 255, 255), 2);
        }
#endif
    }
    else //没有内层装甲板，直接在外层hierarchy查找
    {
#ifdef TRACK               
        if (this->is_tracking) centerP = rect_tmp2.center + offset;
        else centerP = rect_tmp2.center;

        // cout << centerP << endl;  // For Debug
#else
        centerP = rect_tmp2.center;
#endif
        if (this->template_dir == 0)
        {
            if (rect_tmp2.size.height > rect_tmp2.size.width)
            {
                centerP.x -= rect_tmp2.size.height * cos((rect_tmp2.angle + 90)* PI /180) * 0.4;
                centerP.y -= rect_tmp2.size.height * sin((rect_tmp2.angle + 90) * PI / 180) * 0.4;
            }
            else
            {
                centerP.x -= rect_tmp2.size.width * sin((rect_tmp2.angle + 90) * PI / 180) * 0.4;
                centerP.y += rect_tmp2.size.width * cos((rect_tmp2.angle + 90) * PI / 180) * 0.4;
            }
            // cout << "1. "<<centerP << endl;  // For Debug
        }
        else
        {
            if (rect_tmp2.size.height > rect_tmp2.size.width)
            {
                centerP.x += rect_tmp2.size.height * cos((rect_tmp2.angle + 90) * PI / 180) * 0.4;
                centerP.y += rect_tmp2.size.height * sin((rect_tmp2.angle + 90) * PI / 180) * 0.4;
            }
            else
            {
                centerP.x += rect_tmp2.size.width * sin((rect_tmp2.angle + 90) * PI / 180) * 0.4;
                centerP.y -= rect_tmp2.size.width * cos((rect_tmp2.angle + 90) * PI / 180) * 0.4;
            }
            // cout << "2. "<<centerP << endl;  // For Debug
        }

        // cout << "3. "<<centerP << endl;  // For Debug
#ifdef DEBUG
        Mat tmp = srcImage;
        circle(tmp, centerP, 1, Scalar(0, 0, 255), 3);
        namedWindow("debug1", WINDOW_NORMAL);
        imshow("debug1", tmp);
        //waitKey(0);
#endif
    
    }

}

// void PowerRuneDetector::circleAndCurveFitting(bool clearflag, Point2f centerP, 
//                                         Point2f& predictArmorBoxCenter, Point2f delta_p, Mat& drawcircle)
void PowerRuneDetector::circleFitting(bool clearflag, Point2f centerP, 
                                        Point2f delta_p, Mat& drawcircle)
{
    // 拟合圆
    if (clearflag) this->cirV.clear(), this->predV.clear();
    //cout<<"CP"<<centerP<<endl;

    Point2f newArmorBoxCenter = centerP + delta_p; //加上delta_p云台修正
    this->cirV.push_back(newArmorBoxCenter);

    chrono::steady_clock::time_point t1, t2; 

    ////// 丢掉一开始的 pop_n 个数据点
    static bool is_first_data_poped = false; 
    int pop_n = 10;
    if(this->cirV.size() < circleFitDataNumThresh && this->cirV.size() >= pop_n && !is_first_data_poped)
    {
        for(int i =0; i < pop_n; i++)
            this->cirV.pop_front();
        is_first_data_poped = true; 
    }
    ///////
    
    if (this->cirV.size() >= circleFitDataNumThresh)
    { 
        is_first_data_poped = false; 
        
        // printf("this->cirV.size() >= %d\n", circleFitDataNumThresh);
        t1 = chrono::steady_clock::now();
        CircleInfo2(this->cirV, this->cc, this->R);
        t2 = chrono::steady_clock::now();
        // cout << "CircleInfo2 time: " << (static_cast<chrono::duration<double, std::milli>>(t2 - t1)).count() << " ms" << endl;

        if (this->cirV.size() >= this->circleFitDataNumUpperBound) this->cirV.pop_front();

#ifdef SHOW_CIRCLE
        circle(drawcircle, this->cc + delta_p, 1, Scalar(255, 0, 0), 2);                 
        circle(drawcircle, newArmorBoxCenter, 1, Scalar(0, 0, 255), 3); //希望画圆的时候让圆的位置不变
        cout << "centerP: " << centerP << endl; 
        cout << "centerP + delta_p: " << newArmorBoxCenter << endl; 
#endif
    }
    //printf("cirV size is %d predV size is %d\n",this->cirV.size(), this->predV.size());
}

void PowerRuneDetector::getTargetROI(RotatedRect& rect_tmp2, Mat& srcImage, Point2f offset)
{
#ifdef TRACK
    //TODO 此部分需要独立为一个函数：ROI的裁剪
    double tmp_x, tmp_y, tmp_width, tmp_height;
    const double scale_factor = 2; 
    tmp_width = scale_factor * (rect_tmp2.size.width * sin((rect_tmp2.angle + 90) * PI / 180) + rect_tmp2.size.height * cos((rect_tmp2.angle + 90) * PI / 180));
    tmp_height = scale_factor * (rect_tmp2.size.width * cos((rect_tmp2.angle + 90) * PI / 180) + rect_tmp2.size.height * sin((rect_tmp2.angle + 90) * PI / 180));
    tmp_x = rect_tmp2.center.x - 0.5 * tmp_width;
    tmp_y = rect_tmp2.center.y - 0.5 * tmp_height;
    Rect tmp(tmp_x, tmp_y, tmp_width, tmp_height);

    if (!this->is_tracking)
    {
        this->is_tracking = true;
        this->bbox = Rect( clip(tmp.x - tmp.width / 4, 0, srcImage.size[1] - 1.5 * tmp.width),
            clip(tmp.y - tmp.height / 4, 0, srcImage.size[0] - 1.5 * tmp.height),
            1.5 * tmp.width, 1.5 * tmp.height);
    }
    else
    {
        this->bbox = Rect(clip(tmp.x - tmp.width / 4 + offset.x, 0, srcImage.size[1] - 1.5 * tmp.width),
            clip(tmp.y - tmp.height / 4 + offset.y, 0, srcImage.size[0] - 1.5 * tmp.height),
            1.5 * tmp.width, 1.5 * tmp.height);
    }
    
#ifdef SHOW_RESULT
    rectangle(srcImage, this->bbox, Scalar(0, 255, 255), 2);
#endif

#else
    if (!this->is_tracking) this->is_tracking = true;
#endif             
}

bool PowerRuneDetector::detect(Mat srcImage, bool predict_mode, double delay, double delta_x, double delta_y, Point2f& detectPoint, Point2f &predictPoint)
{
        static int loop_count = 0;
        static int last_find = 0;
        static bool clearflag = false;
        static Point2f target;
        static Point2f delta_p;
        bool is_detect_success = false; 

        loop_count++;
        delta_p = Point2f(delta_x, delta_y); //是下位机发来的云台的移动
#ifdef DEBUG
        // printf("delta_X = %lf, delta_y = %lf \n", delta_x, delta_y);
#endif
        // delta_p = Point2f(0.,0.);
        Mat img = srcImage;

        chrono::steady_clock::time_point t1, t2, t3, t4, t5, t6,t7,t8;
        static Mat drawcircle = Mat(srcImage.rows, srcImage.cols, CV_8UC3, Scalar(0, 0, 0));
#ifdef DEBUG
        static int img_count = 0;
#endif
#ifdef SHOW_CIRCLE
        // static Mat drawcircle = Mat(srcImage.rows, srcImage.cols, CV_8UC3, Scalar(0, 0, 0));
#endif
#ifdef SAVE_VIDEO
        if (!writer.isOpened())
        {
            cout << "Capture failed." << endl;
            continue;
        }
        if (isRecording)
        {
            writer << srcImage;
        }
        if (!isRecording)
            cout << "Start capture. " + fileName + " created." << endl;
        isRecording = true;
#endif  
#ifdef TRACK
        if (!this->is_tracking) img = srcImage;
        else img = srcImage(this->bbox);
#endif
        preprocess(img);
        Mat midImage2 = img; 

        //查找轮廓
        vector<vector<Point>> contours2;
        vector<Vec4i> hierarchy2;

        // printf("======================\n");
        t1 = chrono::steady_clock::now();
        findContours(midImage2, contours2, hierarchy2, RETR_TREE, CHAIN_APPROX_NONE); //TODO 在图片中没有轮廓时这一步时间是多少？
        t2 = chrono::steady_clock::now();
        // cout << "findContours time: " << (static_cast<chrono::duration<double, std::milli>>(t2 - t1)).count() << " ms" << endl;

        RotatedRect rect_tmp2;

        //遍历轮廓
        if (hierarchy2.size())
        {
            int i = 0;
            for (; i >= 0; i = hierarchy2[i][0]) //TODO 此部分需要独立为一个函数: 扇叶叶片的提取与矫正
            {
                //rect_tmp2 = minAreaRect(contours2[i]); // 最小包围矩形
                auto rect_tmp21 = minAreaRect2(contours2[i]); // 最小包围矩形
                //printf("method 1 [%f,%f,%f],method 2 [%f,%f,%f]\n",rect_tmp2.center.x, rect_tmp2.center.y,rect_tmp2.angle, rect_tmp21.center.x, rect_tmp21.center.y,rect_tmp21.angle);
                rect_tmp2  = rect_tmp21;

                Point2f P[4];
                rect_tmp2.points(P);

                double width;
                double height;

                //矫正提取的叶片的宽高
                width = getDistance(P[0], P[1]);
                height = getDistance(P[1], P[2]);
#ifdef SHOW_ALL_CONTOUR
                Scalar color(rand() & 255, rand() & 255, rand() & 255);
                drawContours(img, contours2, i, color, 4, 8, hierarchy2);
#endif
                //通过面积筛选
                double area = height * width;
                //if (area > 3000 && area < 5500)
                const double area_thresh = 5000; 
                //////////////////////
                if(area>area_thresh)
                { 
                    Mat leaf_img;
                    if(false == getLeafImage(P, width, height, midImage2, leaf_img))
                    {
#ifdef DEBUG
                        cout << "getLeafImage: failed open" << endl;
#endif
                        is_detect_success = false; 
                        return is_detect_success;
                    }
                    
// #ifdef SAVE_LEAF_IMG
//                     //用于保存扇叶图片
//                     string s = "template" + to_string(cnnt) + ".jpg";
//                     cnnt++;
//                     imwrite("D:/PowerRune/RM19windmillDemo-master/img/" + s, leaf_img);
// #endif

#ifdef DEBUG
                    namedWindow("leaf_img", WINDOW_NORMAL);
                    imshow("leaf_img", leaf_img);
#endif  
                    t1 = chrono::steady_clock::now();
                    leafImageTemplateMatch(leaf_img);
                    t2 = chrono::steady_clock::now();
                    // cout << "leafImageTemplateMatch time: " << (static_cast<chrono::duration<double, std::milli>>(t2 - t1)).count() << " ms" << endl;
                    //Ԥ���Ƿ���Ҫ�������Ҷ

                    // find target power rune armor box 
                    if (this->template_max > this->thr)
                    {
                        // getPowerRuneArmorBoxCenter
                        //cout << "Area: " << area << endl;
                        Point2f centerP;
                        Point2f offset;

                        t1 = chrono::steady_clock::now();
                        getPowerRuneArmorBoxCenter(hierarchy2[i], contours2, rect_tmp2, centerP, offset, img, srcImage);
                        t2 = chrono::steady_clock::now();
                        // cout << "getPowerRuneArmorBoxCenter time: " << (static_cast<chrono::duration<double, std::milli>>(t2 - t1)).count() << " ms" << endl;
                        
                        detectPoint = centerP;
                        // cout << "4. "<< centerP << endl; 
#ifdef SHOW_RESULT
                        circle(srcImage, centerP, 1, Scalar(0, 0, 255), 3);
#endif
                        Point2f newArmorBoxCenter = centerP + delta_p; //加上delta_p云台修正

                        t1 = chrono::steady_clock::now();
                        circleFitting(clearflag, centerP, delta_p, drawcircle);
                        t2 = chrono::steady_clock::now();
                        cout << "circleFitting time: " << (static_cast<chrono::duration<double, std::milli>>(t2 - t1)).count() << " ms" << endl;

                        if (is_tracking || this->predV.size() == 0) 
                            this->predV.push_back(centerP + delta_p);
                        else if(this->predV.size() > 1) 
                        {
                            this->predV.pop_front();
                            this->predV.push_back(target+delta_p); //TODO
                            this->predV.push_back(centerP+delta_p);
                        }
                        if (this->predV.size() >= this->predictDataNumThresh)
                        {
                            t1 = chrono::steady_clock::now();
                            this->predict(predict_mode, delay, this->cirV, target, clearflag);
                            t2 = chrono::steady_clock::now();
                            // cout << "predict time: " << (static_cast<chrono::duration<double, std::milli>>(t2 - t1)).count() << " ms" << endl;
                            predictPoint = target - delta_p;
                            is_detect_success = true;
                         }
                         if(this->predV.size() >= this->predictDataNumUpperBound)
                            this->predV.pop_front();
#ifdef SHOW_RESULT
                        circle(srcImage, predictPoint, 1, Scalar(0, 255, 0), 15);
#endif
// #endif
                        t1 = chrono::steady_clock::now();
                        getTargetROI(rect_tmp2, srcImage, offset);
                        t2 = chrono::steady_clock::now();
                        // cout << "getTargetROI time: " << (static_cast<chrono::duration<double, std::milli>>(t2 - t1)).count() << " ms" << endl;
                        break;
                    }
                }
                ////////////////////// area < area_thresh 
                if (this->is_tracking && hierarchy2[i][0] < 0) // no inner contour 
                {
                    this->is_tracking = false;
#ifdef DEBUG
                    cout << "Failed match: " << this->template_max << endl;
                    cout << "loop count:" << loop_count << endl;
                    Mat debug = srcImage;
                    rectangle(debug, this->bbox, Scalar(0, 255, 255), 2);
                    namedWindow("debug", WINDOW_NORMAL);
                    imshow("debug", debug);
                    // string s = "template" + to_string(img_count) + ".jpg";
                    // imwrite("D:/PowerRune/RM19windmillDemo-master/debug_img/" + s, leaf_img);
                    // img_count++;
                    waitKey(1);
#endif
                }   
            }
        }
        
        if (this->is_tracking) last_find = loop_count, clearflag = false; 
        else if (loop_count - last_find > 5) clearflag = true; // 维护了一个clearflag的状态机

#ifdef SHOW_CIRCLE
        Mat drawcircle2 = drawcircle.clone();
        circle(drawcircle2, this->cc, this->R, Scalar(60, 120, 180), 4);
        //std::cout<<this->cc<<" "<<this->R<<endl;
        namedWindow("circle", WINDOW_NORMAL);
        imshow("circle", drawcircle2);
#endif
#ifdef SHOW_RESULT
        namedWindow("Result", WINDOW_NORMAL);
        imshow("Result", srcImage);
#endif
#if defined SHOW_RESULT || defined SHOW_CIRCLE
        waitKey(1);
#endif
        //if (!this->is_tracking) waitKey(0);

        t6 = chrono::steady_clock::now();
//         cout << "Total period: " << (static_cast<chrono::duration<double, std::milli>>(t6 - t1)).count() << " ms" << endl;
// #ifdef TRACK
//        cout << "Track: " << (static_cast<chrono::duration<double, std::milli>>(t2 - t1)).count() << " ms" << endl;
//        cout << "Preprocess: " << (static_cast<chrono::duration<double, std::milli>>(t3 - t2)).count() << " ms" << endl;
// #else
//        cout << "Preprocess: " << (static_cast<chrono::duration<double, std::milli>>(t3 - t1)).count() << " ms" << endl;
// #endif
//        cout << "Findcontours: " << (static_cast<chrono::duration<double, std::milli>>(t4 - t3)).count() << " ms" << endl;
//        cout << "TemplateMatch: " << (static_cast<chrono::duration<double, std::milli>>(t5 - t4)).count() << " ms" << endl;
//        cout << "FindArmors: " << (static_cast<chrono::duration<double, std::milli>>(t6 - t5)).count() << " ms" << endl;
//         cout << endl;
        return is_detect_success; 
}
